/**
  ******************************************************************************
  * File Name          : motor_bsp_stm32.c
  * Description        : 电机控制代码与底层的交互，即电机的底层代码
                     
										 
	在使用不同电机时，需要用户重写此文件。
	这里使用的是2006电机。
  ******************************************************************************
  */
#include "motor_bsp_stm32.h"
#include "cmsis_os.h"
#include "tim.h"
#include "gpio.h"
#include "math.h"
#include "can.h"
#include "leg.h"


void Motor_Calc_AbsAngle(Motor_t *motor)
{
	float absangle = (motor->bsp.angle + motor->bsp.turns*8192) /819.2f/2.f;
	motor->bsp.abs_angle_fdb = absangle;
}

/**
  * @brief :  ÅÐ¶ÏÊÇÄÄ¸öµç»úµÄÊý¾Ý£¬µ÷ÓÃ¶ÔÓ¦Êý¾Ý´¦Àíº¯Êý
  * @param :  msg:can½ÓÊÕµ½µÄÐÅÏ¢  
  * @retval:  None
  * @note  :  None
  */
void CanReceiveMsgProcess(CAN_RxHeaderTypeDef* RxHeader, uint8_t* RxData)
{
	for (int leg_index = 0; leg_index < 4; leg_index++)
	{
		if(leg[leg_index].m1.bsp.can_id == RxHeader->StdId - 0x200)
		{
			Can_Decode(RxData, &leg[leg_index].m1);
			Count_Truns(&leg[leg_index].m1);
			Motor_Calc_AbsAngle(&leg[leg_index].m1);
		}
		if(leg[leg_index].m2.bsp.can_id == RxHeader->StdId - 0x200)
		{
			Can_Decode(RxData, &leg[leg_index].m2);
			Count_Truns(&leg[leg_index].m2);
			Motor_Calc_AbsAngle(&leg[leg_index].m2);
		}
	}
}


/**
  * @brief :  ½«»ñÈ¡µÄµç»úÊý¾Ý´æ´¢µ½¶ÔÓ¦½á¹¹ÌåÖÐ
  * @param :  msg:can½ÓÊÕµ½µÄÐÅÏ¢  motor£º¶ÔÓ¦µç»ú½á¹¹Ìå
  * @retval:  None
  * @note  :  None
  */
void Can_Decode(uint8_t* RxData, Motor_t *motor)
{
	float speed;
	int16_t temp;
	motor->bsp.angle = RxData[0] << 8 | RxData[1];
	motor->bsp.speed = RxData[2] << 8 | RxData[3];
	motor->bsp.current = RxData[4] <<8 | RxData[5];
	motor->bsp.temp = RxData[6];
	//½«ËÙ¶ÈµÄµ¥Î»×ª»»Îª¡ã/s ¸³¸øfdb
	temp = RxData[2] << 8 | RxData[3];
	speed = (float)temp;
	motor->spd_pid.fdb = speed* 8192 / 60 / 1000;
}

/**
  * @brief :  ¼ÆËãµç»ú×ª¹ýµÄÈ¦Êý
  * @param :  landing_motor£º¶ÔÓ¦µÇÂ½µç»ú½á¹¹Ìå
  * @retval:  None
  * @note  :  None
  */
void Count_Truns(Motor_t *landing_motor)
{
	int16_t diff = landing_motor->bsp.angle - landing_motor->bsp.last_angle;
	//¼ÆËãÈ¦Êý
	if(diff<-6000)           //ÕýÏò×ª¹ýÒ»È¦ cnt++
	{
		landing_motor->bsp.turns++;
	}
	else if(diff>6000)      //·´Ïò×ª¹ýÒ»È¦ cnt--
	{
		landing_motor->bsp.turns--;
	}
	landing_motor->bsp.last_angle = landing_motor->bsp.angle;
}


/**
  * @brief  Rx Fifo 0 message pending callback
  * @param  hcan: pointer to a CAN_HandleTypeDef structure that contains
  *         the configuration information for the specified CAN.
  * @retval None
  */
void HAL_CAN_RxFifo0MsgPendingCallback(CAN_HandleTypeDef *hcan)
{
	CAN_RxHeaderTypeDef   RxHeader;
	uint8_t               RxData[8];
	/* Get RX message */
	if (HAL_CAN_GetRxMessage(hcan, CAN_RX_FIFO0, &RxHeader, RxData) != HAL_OK)
	{
		/* Reception Error */
		Error_Handler();
	}
	
	CanReceiveMsgProcess(&RxHeader, RxData);

}

uint8_t               TxData1[8];
uint8_t               TxData2[8];
void Motor_SetPower(Motor_bsp_t *motor_bsp, float power)
{
	if (power > MOTOR_MAX_POWER) power = MOTOR_MAX_POWER;
	if (power < -MOTOR_MAX_POWER) power = -MOTOR_MAX_POWER;
	uint8_t mat_index = ((motor_bsp->can_id-1)%4)*2;//通过电调ID计算对应的数组下标
	
	if (motor_bsp->can_id <= 4)
	{
		TxData1[mat_index] = (uint8_t)((int16_t)power >> 8);
		TxData1[mat_index+1] = (uint8_t)power;
	}
	else
	{
		TxData2[mat_index] = (uint8_t)((int16_t)power >> 8);
		TxData2[mat_index+1] = (uint8_t)power;
	}
}

void Motor_Cmd_Tx()
{
	/* Tx Header */
	CAN_TxHeaderTypeDef   TxHeader1 = {0x200, 0, CAN_ID_STD, CAN_RTR_DATA, 0x08};//对应电调ID 1-4
	CAN_TxHeaderTypeDef   TxHeader2 = {0x1FF, 0, CAN_ID_STD, CAN_RTR_DATA, 0x08};//对应电调ID 5-8
	uint32_t              TxMailbox;

	/* Start the Transmission process 0x200 */
	if (HAL_CAN_AddTxMessage(&hcan1, &TxHeader1, TxData1, &TxMailbox) != HAL_OK)
	{
		/* Transmission request Error */
		Error_Handler();
	}
	
	/* Start the Transmission process 0x1FF */
	if (HAL_CAN_AddTxMessage(&hcan1, &TxHeader2, TxData2, &TxMailbox) != HAL_OK)
	{
		/* Transmission request Error */
		Error_Handler();
	}
}

